
//#pragma once

#ifndef COLLISION_DETECTOR_H
#define COLLISION_DETECTOR_H

#include <math.h>
//#include <iostream>
//#include <assert.h>
//#include "Utils\Rotation_utils\RotationUtils.h"


template <typename K> //K is the kernel type
class CollisionDetector{

public:
	/*
	typedef CGAL::Point_2<K>			Point2D;
	typedef CGAL::Polygon_2<K>			Polygon;
	typedef std::vector<Polygon>		Polygon_vector;
	*/
	typedef typename K::FT						NT; //number type
	typedef typename K::Point_2					Point;
	typedef Rotation<NT>						Rotation;
	typedef CGAL::Polygon_2<K>					Polygon;
	typedef Extended_polygon<K>					Robot_rep;
	typedef Reference_point<K>					ReferencePoint;
	typedef std::vector<ReferencePoint>			Path;
	typedef CGAL::Polygon_set_2<K>				Polygon_set;

	
	
private:
	
	double const		m_step; // step for translation movement
	double const		m_tilt; // step for rotation movement
	Polygon_set			m_obstacles; // workspace polygons

	bool isValidRotation(Robot_rep robot,Rotation const& r1, Rotation const& r2) const
	{
		std::vector<Rotation> container;
		interpolate(r1,r2,container);
		/*for (unsigned int i = 0; i < container.size(); i++){
			Rotation r = container[i];
			std::cout << r.to_angle() << " ";
		} std::cout << endl;*/
		if (container.size() < 3) return false;
		for (unsigned int i = 0; i < container.size(); i++){
			//global_graphics->draw_polygon<K> (robot.get_absolute_polygon(), ORANGE);
			robot.rotate_absolute(container[i]);
			if (collide(robot.get_absolute_polygon())) {
				//global_graphics->draw_polygon<K> (robot.get_absolute_polygon(), RED);
				return false;
			}
		}
		return true;
	}

	bool isValidTranslation(Robot_rep robot,Point const& s, Point const& t) const
	{
		std::vector<Point> container;
		interpolate(s,t,container);
		if (container.size() < 3) return false;
		for (unsigned int i = 0; i < container.size(); i++){
			robot.translate_absolute(container[i]);
			//global_graphics->draw_polygon<K> (robot.get_absolute_polygon(), ORANGE);
			if (collide(robot.get_absolute_polygon())) {
				//global_graphics->draw_polygon<K> (robot.get_absolute_polygon(), RED);
				return false;
			}
		}
		return true;
	}

	bool collide(Polygon const & robot) const
	{
		Polygon_set obstacles = m_obstacles;
		return obstacles.do_intersect(robot);
	}


	void interpolate(Rotation const& r1, Rotation const& r2,std::vector<Rotation>& container) const
	{
		double src = r1.to_angle();
		double dst = r2.to_angle();
		double curr = src;

		double delta = smallest_angle(src,dst);
		double step = (delta < 0) ? -m_tilt : m_tilt;

		//std::cout << "src: " << src << " dst: " << dst << " step: " << step << " delta: " << delta << std::endl;
		
		double rad,s,c;
		Rotation r = r1;
		container.push_back(r1);
		int num_steps = (int) std::abs(delta/step);
		for (int i = 0; i < num_steps; i++){
			curr += step; //step in deg!
			rad = curr*PI/180.0;
			s = std::sin(rad);
			c = std::cos(rad);
			r = Rotation(s,c);
			container.push_back(r);
		}
		container.push_back(r2);
	}



	void interpolate(Point const& s, Point const& t,std::vector<Point>& container) const
	{
		Rotation a = getLineDirection(s,t);
		NT x_step = a.cos()*m_step;
		NT y_step = a.sin()*m_step;

		container.push_back(s);
		double d(distance_points(s,t));
		NT x = s.x(); NT y = s.y(); Point p(s);
		int num_steps = (int) std::abs(d/m_step);
		for (int i = 0; i < num_steps; i++){
			x = x + x_step;
			y = y + y_step;
			p = Point(x,y);
			container.push_back(p);
		}
		container.push_back(t);
	}

	NT slope(Point const& s, Point const& t) const
	{
		NT dx = t.x()-s.x();
		NT dy = t.y()-s.y();
		return dy/dx;
	}

public:

	double distance_points(Point const& s, Point const& t) const
	{
		NT dx = t.x()-s.x();
		NT dy = t.y()-s.y();
		return std::sqrt(CGAL::to_double(dx*dx+dy*dy));
	}

	CollisionDetector (std::vector<Polygon> const& workspace,double step=0.05,double tilt=18.0) : 
	m_step(step),m_tilt(tilt)
	{
		for (unsigned int i = 0; i < workspace.size(); i++){
			add_obstacle(workspace[i]);
		}
	}

	bool isValid (Polygon const & robot_model, ReferencePoint const & s, ReferencePoint const & t,Path& path) const
	{
		ReferencePoint next; Rotation r; 
		Robot_rep robot = Robot_rep(robot_model); 
		robot.translate_absolute(s.get_location());
		r = getLineDirection(s.get_location(),t.get_location());

		if (!isValidRotation(robot,s.get_rotation(),r)) return false;
		robot.rotate_absolute(r);
		next = ReferencePoint(s.get_location(),r);
		path.push_back(next);

		if (!isValidTranslation(robot,s.get_location(),t.get_location())) return false;
		robot.translate_absolute(t.get_location());
		next = ReferencePoint(t.get_location(),r);
		path.push_back(next);

		return isValidRotation(robot,r,t.get_rotation());
	}

	Rotation getLineDirection(Point const & s, Point const & t) const
	{
		NT dx = t.x()-s.x();
		NT dy = t.y()-s.y();
		double norm = std::sqrt(CGAL::to_double(dy*dy+dx*dx));
		return Rotation(dy/norm,dx/norm);
	}

	void add_obstacle(Polygon const & polygon)
	{
		//global_graphics->draw_polygon<K>(polygon,BLUE);
		m_obstacles.join(polygon);
	}
};



static inline double smallest_angle(double s, double t, Angle_type mode = DEG)
{
	double a =  t - s;
	switch(mode)
	{
		case DEG:
			if (a > 180.0)
				a -= 360.0;
			else if (a < -180.0)
				a += 360.0;
			return a;
		case RAD:
			if (a > 1.0*PI)
				a -= 2.0*PI;
			else if (a < -1.0*PI)
				a += 2.0*PI;
			return a;
		default:
			return a;
	}
}


#endif //COLLOSION_DETECTION_H